package trb.complexshield.closedmesh;

import no.hrp.hvrc.rad.calc.intersect.Segment;
import no.hrp.hvrc.rad.calc.intersect.raytrace.CollisionMesh;
import no.hrp.hvrc.rad.calc.intersect.raytrace.Intersection;
import no.hrp.hvrc.rad.calc.intersect.raytrace.RayCollisionPacket;
import no.hrp.hvrc.rad.dosim.bvh.BottumUpBVH;
import no.hrp.hvrc.utils.j3d.BBox;
import trb.complexshield.ComplexShield;

import javax.media.j3d.Transform3D;
import javax.vecmath.Point3f;
import javax.vecmath.Vector3f;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.List;

/**
 * A ComplexShield that uses a closed mesh to find where the ray is inside.
 * Whether the ray starts inside the mesh is determined by counting the number
 * of intersections from the origin along an infinite ray. If there are an odd
 * number of intersections then the origin is inside. The intersection code is
 * optimised using a Bounding Volume Hierarchy.
 */
public class BvhCollisionMesh implements ComplexShield {

    private static final ThreadLocal<RayCollisionPacket> rayThreadLocal = new ThreadLocal<RayCollisionPacket>() {
        @Override
        protected RayCollisionPacket initialValue() {
            return new RayCollisionPacket();
        }
    };

    // the shields local to world matrix
    private final Transform3D local2World = new Transform3D();

    // the world to local space transformation matrix
    protected Transform3D world2local = new Transform3D();

    // temp var, the transformed line
    private static final ThreadLocal<Segment> lThreadLocal = new ThreadLocal<Segment>() {
        @Override
        protected Segment initialValue() {
            return new Segment(new Vector3f(), new Vector3f(0, 1, 0));
        }
    };
    private final BBox aabb;
    private final BottumUpBVH.AABB bvhAabb;

    public final CollisionMesh collisionMesh;
    private final BottumUpBVH bottumUpBVH;


    public BvhCollisionMesh(float[] coords, int[] indices, Transform3D t3d) {
        collisionMesh = new CollisionMesh(coords, indices);
        world2local.invert(t3d);
        local2World.set(t3d);
        aabb = new BBox(coords);
        aabb.transform(local2World);
        bvhAabb = new BottumUpBVH.AABB(aabb);

        List<BottumUpBVH.Leaf> leafs = new ArrayList();

        for (int i=0; i<collisionMesh.getFaceCount(); i++) {
            leafs.add(new TriangleLeaf(i, Arrays.copyOfRange(collisionMesh.AABB, i*6, (i+1)*6)));
        }

        bottumUpBVH = new BottumUpBVH(leafs);
    }

    @Override
    public void intersect(Segment line, Output output) {
        if (!bvhAabb.intersects(line)) {
            return;
        }
        Segment l = lThreadLocal.get();
        l.transform(line, world2local);

        RayCollisionPacket ray = rayThreadLocal.get();

        // get all the intersections between line and mesh
        ray.reset(l.originRef(), l.directionRef());

        List<BottumUpBVH.Leaf> result = new ArrayList();
        bottumUpBVH.intersect(l, result);

        bruteRayCollisionDetection(ray, result);

        // sort intersections on distance from startPos
        ray.sortResults();

        // check if startPos is inside mesh
        boolean startsInside = (ray.results.size() % 2) == 1;
        boolean isInside = startsInside;

        // calculate distances based on intersections and starts inside
        float endTime = l.length();
        float ooEndTime = 1f / endTime;
        float prevTime = 0;
        float curTime = 0;
        for (Intersection intersection : ray.results) {
            float collisionTime = intersection.time;
            curTime = collisionTime;
            if (curTime > endTime) {
                // exit loop if collision is not on line
                break;
            }
            if (isInside) {
                output.addIntersection(prevTime * ooEndTime, collisionTime * ooEndTime);
            } else {
                prevTime = collisionTime;
            }
            isInside = !isInside;
        }

        if (isInside && ((endTime - prevTime) > 0)) {
            output.addIntersection(prevTime * ooEndTime, endTime * ooEndTime);
        }
    }

    /**
     * Do collision detection between the specified line and this mesh. The line
     * coordinates must be in object space.
     * @param ray contains paramters and return values.
     */
    public void bruteRayCollisionDetection(RayCollisionPacket ray, List<BottumUpBVH.Leaf> leafs) {
        Point3f p1 = ray.origin;
        Vector3f d = ray.direction;
        if (d.lengthSquared() == 0) {
            return;
        }

        CollisionMesh.Temp temp = CollisionMesh.tempThreadLocal.get();

        for (int leafIdx=0; leafIdx<leafs.size(); leafIdx++) {
            TriangleLeaf leaf = (TriangleLeaf) leafs.get(leafIdx);
            int faceIdx = leaf.faceIndex;

            // load triangle
            collisionMesh.loadCollisionTriangle(faceIdx, temp.triangle);
            float dist1 = temp.triangle.distanceToPlane(p1);
            float dirDot = d.dot(temp.triangle.normal);

            // check if the ray intersects this nodes plane.
            boolean intersectsPlane = ((dist1 * dirDot) < 0);
            if (intersectsPlane) {
                // get where line intersects plane
                float t = -dist1 / (dirDot);
                temp.intersection.scaleAdd(t, d, p1);

                if (temp.triangle.contains(temp.intersection)) {
                    ray.registerCollision(t, collisionMesh, faceIdx);
                }
            }
        }
    }


    public BBox getAABB() {
        return aabb;
    }

    static class TriangleLeaf implements BottumUpBVH.Leaf {

        private final int faceIndex;
        private final BottumUpBVH.AABB aabb;

        TriangleLeaf(int faceIndex, float[] minMax) {
            this.faceIndex = faceIndex;
            aabb = new BottumUpBVH.AABB(minMax);
            float dx = aabb.minMax[3] - aabb.minMax[0];
            float dy = aabb.minMax[4] - aabb.minMax[1];
            float dz = aabb.minMax[5] - aabb.minMax[2];
            float growScale = 1E-6f;
            aabb.minMax[0] -= dx * growScale;
            aabb.minMax[1] -= dy * growScale;
            aabb.minMax[2] -= dz * growScale;
            aabb.minMax[3] += dx * growScale;
            aabb.minMax[4] += dy * growScale;
            aabb.minMax[5] += dz * growScale;
        }

        @Override
        public BottumUpBVH.AABB getAABB() {
            return aabb;
        }
    }
}


